perm filename SERVO.FAI[CMS,LCS]8 blob
sn#439817 filedate 1979-05-10 generic text, type T, neo UTF8
00100 TITLE SERVO
00200 .INSERT ASMBL.FAI[CMS,LCS]
00300
00400 ;I/O address definitions.
00500 DAC ← 100000 ;8 bit DAC.
00600 JCR ← 120000 ;Joint control output register.
00700 ENCL ← 140000 ;Encoder mux low.
00800 ENCH ← 140001 ;Encoder mux high.
00900
01000 STKSIZ ← 377 ;Stack size.
01100 LSBENB ← 40 ;Enable LSB servo.
01200
01300 ;Zero page variables.
01400 ;Not shared.
01500
01600 DSPAT: BLOCK 2 ;Dispatch address for commands.
01700 DEFCMD: 0 ;Deferred command.
01800 0 ;For 16 bit even addresses.
01900 SAVPOS: BLOCK 2 ;Position for deferred servo command.
02000
02100 CMDVEL: BLOCK 2 ;Commanded velocity.
02200 CURVEL: BLOCK 2 ;Current velocity.
02300 0 ;SETPT-1.
02400 SETPT: BLOCK 2 ;Current setpoint.
02500 0 ;SETINC-1.
02600 SETINC: BLOCK 2 ;Interpolating increment for setpoints.
02700 0 ;INTINC-1.
02800 INTINC: BLOCK 2 ;Interpolating increment for velocity.
02900 OLDVEL: BLOCK 2 ;Last commanded velocity.
03000 OLDSP: BLOCK 2 ;Last commanded setpoint, for CMDVEL.
03100 POSERR: BLOCK 2 ;Current position error.
03200 DACSIG: BLOCK 2 ;Scratch.
03300
03400 INCTR: 0 ;Count the interpolations.
03500 HSTTMR: 0 ;Count ticks between host commands.
03600
03700 LOGTMP: BLOCK 4 ;Temp for the arithmetic routines.
03800
03900 0 ;IVEL-1.
04000 IVEL: BLOCK 2 ;Interpolated velocity.
04100 VELERR: BLOCK 2 ;Velocity error term.
04200 VSUM: BLOCK 2 ;Sum of last 8 velocitys.
04300 VPTR: 0 ;Velocity averaging index.
04400 VELTBL: BLOCK 20;Velocity averaging table.
04500
04600 ZAPEND ← .-1 ;Clear all the above in startup.
04700
04800 TL: 0 ;Scratch for gray to binary.
04900 TH: 0
05000
05100 FTMP: BLOCK 2 ;Copy of friction coefficient for multiply.
00100 ;Shared ram.
00200 LOC 200 ;Second half of zero page.
00300
00400 ;STATUS byte bits.
00500 ; 7 6 5 4 3 2 1 0
00600 ; check time no
00700 ; word out tick
00800
00900 0 ;Locked.
01000 STATUS: 0 ;Flags for the host.
01100
01200 ;MODE byte bits.
01300 ;Bit 7 6 5 4 3 2 1 0
01400 ; servo integ lsb
01500 ; enlb enlb enbl
01600
01700 0 ;Locked.
01800 MODE: 0 ;Mode bits from host.
01900
02000 CKWORD: BLOCK 2 ;Host I/O check/command word.
02100 CMDPOS: BLOCK 2 ;Commanded position from host.
02200
02300 ;IOCTRL byte bits.
02400 ;Bit 7 6 5 4 3 2 1 0
02500 ; in lsb integ pos
02600 ; tol enlb disabl mode
02700
02800 0 ;Locked.
02900 IOCTRL: 0 ;Copy of JCR output port.
03000
03100 CURPOS: BLOCK 2 ;Current position.
03200
03300 ;NINTER = function of INTSCL?
03400 0 ;Locked.
03500 NINTER: 0 ;# of interpolations between position
03600 ;commands.
03700 0 ;Locked.
03800 INTSCL: 0 ;# of bits to shift setpoint dif for
03900 ;interpolating.
04000 0 ;Locked.
04100 HSTLIM: 0 ;# of clock ticks allowed between host
04200 ;commands.
04300 FRICTN: BLOCK 2 ;Viscous damping coefficient.
04400 GRAVTY: BLOCK 2 ;DC offset for gravity.
04500 POSTOL: BLOCK 2 ;Half-width of position tolerance band.
04600 INTTOL: BLOCK 2 ;Half-width of integration band.
04700
04800
04900 ;Start of prom.
05000 LOC 174000
05100
05200 INITBL: STATUS ↔ 0
05300
05400 NINTER ↔ =32
05500 INTSCL ↔ 5
05600
05700 HSTLIM ↔ =48
05800
05900 377 ;End of INITBL flag.
00100 ;Power on reset.
00200 START: CLD
00300 LDXI STKSIZ ;Setup stack.
00400 TXS
00500
00600 LDAI 0
00700 LDXI ZAPEND
00800 RLOOP: STAZX 0 ;Reset ram.
00900 DEX
01000 BPL RLOOP
01100 STA DAC ;Clear DAC.
01200
01300 LDXI 370 ;-8.
01400 ZSR: STAZX FRICTN+10 ;Clear shared ram.
01500 INX
01600 BMI ZSR
01700
01800 TAY ;Y ← 0.
01900 BEQ RSTDEF ;Jump
02000 DLOOP: INY
02100 LDAY INITBL ;Init ram.
02200 STAZX 0
02300 INY
02400 RSTDEF: LDXY INITBL
02500 CPXI 377
02600 BNE DLOOP
02700
02800 STOP: SEI ;Go into stop mode.
02900 JSR GETPOS ;Read encoder and convert to binary.
03000
03100 ;Sets the current position to the converted encoder value, the
03200 ;setpoint the same, clears the setpoint interpolating increment,
03300 ;and goes into stop mode.
03400 STAZ CURPOS ;Set the current position.
03500 STXZ CURPOS+1;Unlock.
03600 STAZ SETPT ;Set the setpoint.
03700 STXZ SETPT+1
03800 STAZ OLDSP ;For CMDVEL.
03900 STXZ OLDSP+1
04000
04100 LDAI 75 ;I/O control bits for servo enable on,
04200 STAZ IOCTRL ;all others off.
04300 STA JCR
04400
04500 LDAI 0
04600 STAZ MODE ;Clear position servo enable, etc..
04700 STAZ DEFCMD ;Clear the deferred command flag.
04800 STAZ SETPT-1 ;Clear the setpoint extension,
04900 STAZ SETINC-1;the interpolator,
05000 STAZ SETINC
05100 STAZ SETINC+1
05200 STAZ CMDVEL ;and the commanded velocity.
05300 STAZ CMDVEL+1
05400 STAZ OLDVEL
05500 STAZ OLDVEL+1
05600
05700 LDXI 27
05800 CLRVEL: STAZX IVEL-1 ;Clear velocity values.
05900 DEX
06000 BPL CLRVEL
06100 CLI ;End of reset.
00100 RSTCKW: LDAI 377 ;Reset check word.
00200 LDXI 0
00300 SEI
00400 STAZ CKWORD ;Lock.
00500 STXZ CKWORD+1;Unlock.
00600 CLI
00700 ;Idle loop. Wait for command.
00800 IDLE: LDAZ CKWORD+1;Check for new check word.
00900 BEQ IDLE ;Not equal if bit 7 is complement of low byte.
01000
01100 SEC
01200 SEI
01300 ADCZ CKWORD ;Lock.
01400 LDXZ CKWORD+1;Unlock.
01500 CLI
01600 TAY
01700 BNE CKWDER ;Check word error.
01800 ;Check here for immediate or deferred.
01900 TXA ;Check for valid command.
02000 ORAI 3 ;3 for two commands and bit 0 = 0.
02100 ADCI 0 ;Carry = 1.
02200 BNE CKWDER ;Not a valid command.
02300
02400 LDAZ DEFCMD ;Check if no TICK?
02500 BNE NOTICK ;No response since last deferred command.
02600
02700 SEI
02800 LDYZ CMDPOS ;Read position for servo command.
02900 LDAZ CMDPOS+1;Unlock.
03000 CLI
03100
03200 STYZ SAVPOS ;Save it for later.
03300 STAZ SAVPOS+1
03400 ASLA ;Check for valid position.
03500 BCS CSET
03600 BMI CKWDER
03700 BPL GOODP
03800 CSET: BPL CKWDER
03900
04000 GOODP: STXZ DEFCMD ;Save deferred command pointer.
04100
04200 JMP RSTCKW ;Handshake with host via CKWORD.
04300
04400 CKWDER: LDAI 100 ;Set check word error flag.
04500 WSTAT: ORAZ STATUS
04600 STAZ STATUS
04700 JMP STOP
04800
04900 ;If status is in low byte. For host lockout.
05000 ; LDAI 100
05100 ; SEI
05200 ; ORAZ STATUS
05300 ; STAZ STATUS
05400 ; LDAZ STATUS+1
05500 ; CLI
05600 ; JMP STOP
05700
05800 NOTICK: LDAI 20 ;Set tick error flag.
05900 BNE WSTAT ;Jump.
00100 ;Clock tick interrupt.
00200 TICK: PHA ;Save state.
00300 TXA
00400 PHA
00500 TYA
00600 PHA
00700
00800 INCZ IOCTRL ;Turn interrupt flag bit.
00900 LDAZ IOCTRL
01000 STA JCR
01100
01200 JSR GETPOS ;Read position and convert to binary.
01300 SEC
01400 SBCZ CURPOS ;Subtract the old position
01500 STAZ CURVEL ;yielding the velocity.
01600 TXA ;High byte of binary position.
01700 SBCZ CURPOS+1;Unlock.
01800 STAZ CURVEL
01900
02000 STYZ CURPOS ;Update the current position.
02100 STXZ CURPOS+1;Unlock.
02200 DECZ HSTTMR ;Count the ticks since the last command
02300 BPL HOSTOK ;and check for timeout.
02400
02500 LDAI 0 ;Host dead. Stop.
02600 STAZ HSTTMR
02700 STAZ CMDVEL
02800 STAZ CMDVEL+1
02900 STAZ INTINC-1;?
03000 STAZ INTINC
03100 STAZ INTINC+1
03200 LDAI 40 ;Set host time out flag
03300 ORAZ STATUS
03400 STAZ STATUS
03500
03600 HOSTOK: LDAI 4
03700 BITZ IOCTRL ;If position mode is off,
03800 BNE INTVEL
03900 JMP CURSRV ;don't servo.
04000
04100 INTVEL: CLC ;Interpolate the velocity.
04200 LDAZ IVEL-1
04300 ADCZ INTINC-1;IVEL ← IVEL + INTINC.
04400 STAZ IVEL-1
04500 LDAZ IVEL
04600 ADCZ INTINC
04700 STAZ IVEL
04800 LDAZ IVEL+1
04900 ADCZ INTINC+1
05000 STAZ IVEL+1
00100 ;Interpolate the setpoints.
00200 INTRS: CLC
00300 LDAZ SETPT-1
00400 ADCZ SETINC-1;Add the increment to the setpoint.
00500 STAZ SETPT-1
00600 LDAZ SETPT
00700 ADCZ SETINC
00800 STAZ SETPT
00900 LDAZ SETPT+1
01000 ADCZ SETINC+1
01100 STAZ SETPT+1
01200
01300 DECZ INCTR ;Check if this is the last interpolation.
01400 BNE GPOSER
01500
01600 LDAI 0 ;Clear SETINC if done interpolating.
01700 STAZ SETINC-1
01800 STAZ SETINC
01900 STAZ SETINC+1
02000 STAZ INTINC-1
02100 STAZ INTINC
02200 STAZ INTINC+1
02300
02400 ;Calculate the position error.
02500 GPOSER: SEC
02600 LDAZ CURPOS ;POSERR ← CURPOS - SETPT.
02700 SBCZ SETPT
02800 STAZ POSERR
02900 LDAZ CURPOS+1;Unlock.
03000 SBCZ SETPT+1
03100 STAZ POSERR+1
00100 BITZ MODE ;If servo is disabled, we're
00200 BPL OOTOL ;automatically out of tolerance
00300
00400 LDAZ POSERR+1;Test the sign of pos error.
00500 BMI NEGPER
00600
00700 LDAZ POSTOL ;Positive. Compare with tol.
00800 CMPZ POSERR
00900 LDAZ POSTOL+1;Unlock.
01000 SBCZ POSERR+1
01100 BCS TOLOK ;In tolerance.
01200 BCC OOTOL ;Jump.
01300
01400 NEGPER: CLC ;Negative. Add the tolerance.
01500 LDAZ POSTOL ;Lock.
01600 ADCZ POSERR
01700 LDAZ POSTOL+1;Unlock.
01800 ADCZ POSERR+1
01900 BCS TOLOK ;In tolerance.
02000
02100 OOTOL: LDAZ IOCTRL ;Out of tolerance.
02200 ANDI 177 ;Turn off the in tolerance
02300 BNE WCNTRL ;indicator. Jump.
02400
02500 TOLOK: LDAZ IOCTRL ;In tolerance. Turn it on.
02600 ORAI 200
02700 WCNTRL: STAZ IOCTRL
02800 STA JCR ;Copy it to output.
02900
03000 BITZ MODE ;If intergration is disabled,
03100 BVC OOBAND ;turn it off.
03200 LDAZ POSERR+1;Test sign of position error.
03300 BMI ADTOL
03400
03500 LDAZ INTTOL ;Positive. Compare with tol.
03600 CMPZ POSERR
03700 LDAZ INTTOL+1;Unlock.
03800 SBCZ POSERR+1
03900 BCS INBAND ;In band. Turn on integrator.
04000 BCC OOBAND ;Jump.
04100
04200 ADTOL: CLC ;Negative. Add the tolerance.
04300 LDAZ INTTOL ;Lock.
04400 ADCZ POSERR
04500 LDAZ INTTOL+1;Unlock.
04600 ADCZ POSERR+1
04700 BCS INBAND ;Check if in band.
04800
04900 OOBAND: LDAZ IOCTRL ;Out of band. Turn off
05000 ORAI 10 ;integration by setting the
05100 ANDI 357 ;control bit. LSB servo off.
05200 BNE WCTRL2 ;Jump.
00100 INBAND: LDAI LSBENB ;In band. Is LSB servo enabled?
00200 BITZ MODE
00300 BEQ RCNTRL
00400
00500 LDAZ POSERR ;Yes. Is the error exactly 0?
00600 ORAZ POSERR+1
00700 BNE RCNTRL
00800
00900 LDAZ IOCTRL ;It is. Integration off, LSB
01000 ORAI 30 ;servo on.
01100 BNE WCTRL2 ;Jump.
01200
01300 RCNTRL: LDAZ IOCTRL ;LSB disabled or error
01400 ANDI 347 ;not zero. LSB servo off,
01500 ;integration on.
01600
01700 WCTRL2: STAZ IOCTRL
01800 STA JCR ;Output it.
01900
02000 ;Get the velocity error.
02100 CLC
02200 LDAZ VSUM
02300 ADCZ CURVEL ;VSUM ← VSUM + CURVEL.
02400 TAX
02500 LDAZ VSUM+1
02600 ADCZ CURVEL+1
02700 TAY
02800 TXA
02900 LDXZ VPTR ;Get velocity averaging index.
03000 SEC
03100 SBCZX VELTBL ;VSUM ← VSUM - VELTBL[VPTR].
03200 STAZ VSUM
03300 TYA
03400 SBCZX VELTBL+10
03500 STAZ VSUM+1
03600
03700 LDAZ CURVEL ;VELTBL[VPTR] ← CURVEL.
03800 STAZX VELTBL
03900 LDAZ CURVEL+1
04000 STAZX VELTBL+10
04100 INX ;VPTR ← (VPTR + 1) .AND. (VTLEN - 1).
04200 TXA
04300 ANDI 7
04400 STAZ VPTR
04500
04600 SEC
04700 LDAZ VSUM
04800 SBCZ IVEL ;VELERR ← VSUM - IVEL.
04900 STAZ VELERR
05000 LDAZ VSUM+1
05100 SBCZ IVEL+1
00100 LDYZ VELERR ;Get the velocity error,
00200 ;A = VELERR+1
00300 JSR LOG
00400 LDXZ FRICTN ;(Copy friction for multiply.)
00500 STXZ FTMP
00600 LDXZ FRICTN+1;Unlock.
00700 STXZ FTMP+1
00800 LDXI FTMP ;multiply by the friction
00900 JSR MULTIP ;coefficient,
01000 JSR EXP
01100
01200 TAX ;Save high byte.
01300 TYA ;Get low byte.
01400 CLC ;add the position error...
01500 ADCZ POSERR
01600 STAZ DACSIG
01700 TXA
01800 ADCZ POSERR+1
01900 STAZ DACSIG+1
02000
02100 CLC ;...and the gravity offset.
02200 LDAZ DACSIG
02300 ADCZ GRAVTY ;Lock.
02400 TAY ;Save low byte.
02500 LDAZ GRAVTY+1;Unlock.
02600 ADCZ DACSIG+1
02700
02800 JSR PUTDAC ;Put result out to the DAC.
02900
03000 CMDSP: LDAZ DEFCMD ;Check for a command.
03100 BEQ INTXIT
03200 ANDI 2 ;Low nibble command bit.
03300 TAX
03400 LDAX CMDTBL ;Get command address.
03500 STAZ DSPAT
03600 LDAX CMDTBL+1
03700 STAZ DSPAT+1
03800 JMPIN DSPAT ;Execute command.
03900
04000 CMDEND: LDAI 0 ;Done with deferred command.
04100 STAZ DEFCMD ;Reset command word.
04200 INTXIT: DECZ IOCTRL ;Turn off interrupt flag.
04300 LDAZ IOCTRL
04400 STA JCR
04500 PLA ;Restore state and dismiss interrupt.
04600 TAY
04700 PLA
04800 TAX
04900 PLA
05000 RTI
05100
05200 CMDTBL: ;DEFERRED COMMAND TABLE.
05300 CMDEND∧377 ;Nop.
05400 (CMDEND⊗-10)∧377
05500 CMDSRV∧377 ;Servo command.
05600 (CMDSRV⊗-10)∧377
00100 ;Deferred commands.
00200 CMDSRV: LDAZ MODE ;Servo command.
00300 BMI ENBLD ;Test for servo enabled.
00400 JMP CMDEND ;No. End this command.
00500
00600 ENBLD: LDAZ SAVPOS ;Enabled. Get commanded position.
00700 SEC
00800 SBCZ SETPT ;Get differance between next position
00900 STAZ SETINC ;and the last setpoint.
01000 LDAZ SAVPOS+1
01100 SBCZ SETPT+1
01200 LDXI 0
01300 STXZ SETPT-1 ;Clear setpoint and increment extentions.
01400 STXZ SETINC-1
01500 STXZ INTINC-1
01600 LDXZ INTSCL
01700
01800 SCAL: CMPI 200 ;Extend sign.
01900 RORA ;Divide the differance by the number of interpolations.
02000 RORZ SETINC
02100 RORZ SETINC-1
02200 DEX
02300 BNE SCAL
02400
02500 STAZ SETINC+1;Which yields the interpolating increment.
02600 LDAZ NINTER
02700 STAZ INCTR ;Setup the interpolator count.
02800 SEC
02900 LDAZ SAVPOS
03000 SBCZ OLDSP
03100 STAZ CMDVEL ;CMDVEL ← CMDPOS - OLDSP.
03200 LDAZ SAVPOS+1
03300 SBCZ OLDSP+1
03400 STAZ CMDVEL+1
03500 LDAZ SAVPOS
03600 STAZ OLDSP ;OLDSP ← CMDPOS.
03700 LDAZ SAVPOS+1
03800 STAZ OLDSP+1
03900 ;INTINC ← (CMDVEL - OLDVEL) / NINTER ?
04000 ;OLDVEL ← CMDVEL.
04100 SEC
04200 LDAZ CMDVEL
04300 SBCZ OLDVEL ;INTINC ← CMDVEL - OLDVEL.
04400 STAZ INTINC
04500 LDAZ CMDVEL+1
04600 SBCZ OLDVEL+1
04700 CMPI 200 ;Extend sign and divide by 4.
04800 RORA
04900 RORZ INTINC
05000 RORZ INTINC-1
05100 CMPI 200
05200 RORA
05300 RORZ INTINC
05400 RORZ INTINC-1
05500
05600 LDAZ CMDVEL
05700 STAZ OLDVEL ;OLDVEL ← CMDVEL.
05800 LDAZ CMDVEL+1
05900 STAZ OLDVEL+1
00100 LDAZ IOCTRL
00200 ORAI 44 ;Turn on servo and current mode enable bits.
00300 STAZ IOCTRL
00400 STA JCR ;Output it.
00500 LDAZ HSTLIM ;Reset host timer.
00600 STAZ HSTTMR
00700 JMP CMDEND
00800
00900 ;Free mode.
01000 CURSRV: LDAI 0 ;Not servoing ("Current mode")...
01100 STAZ SETPT-1 ;Make the setpoint track
01200 LDAZ CURPOS ;the current position in order to
01300 STAZ SETPT ;keep the arm from twitching when
01400 LDAZ CURPOS+1;the host enables the servo. Unlock.
01500 STAZ SETPT+1
01600 JMP CMDSP ;Go check on commands.
01700
01800 ;DAC output subroutine.
01900 ;Enter with 2 byte value in Y (low), A (high).
02000 ;Clobbers all registers, but the 8 bits the DAC got are returned in A.
02100 PUTDAC: BMI NEGDAC ;Assuming the last inst. loaded A.
02200 CPYI 200 ;Positive. Compare with 2↑7.
02300 SBCI 0
02400 BCC INRNGE
02500
02600 TOOHI: LDYI 177 ;Too high. Saturate positive.
02700 BNE INRNGE ;Jump.
02800
02900 NEGDAC: CPYI 200 ;Negative. Compare with -2↑7.
03000 SBCI 377
03100 BCS INRNGE
03200
03300 TOOLOW: LDYI 200 ;Too low. Saturate to -2↑7.
03400
03500 INRNGE: LDAY VETBL ;Straighting it.
03600 STA DAC ;Output 8 bits to the DAC.
03700 RTS
00100 ;Position conversion routine.
00200 GETPOS: LDY ENCL ;Read encoder.
00300 LDA ENCH
00400 EORI 377 ;Complement it.
00500 ;Convert from gray to binary.
00600 STAZ TH
00700 LSRA ;Shift by 1.
00800 EORZ TH
00900 STAZ TH
01000 TAX ;X ← high byte.
01100
01200 TYA
01300 EORI 377 ;Complement low byte.
01400 STAZ TL
01500 RORA
01600 EORZ TL
01700 STAZ TL
01800
01900 LSRZ TH ;Shift by 2.
02000 RORA
02100 LSRZ TH
02200 RORA
02300 EORZ TL
02400 STAZ TL
02500 TAY ;Y ← low byte.
02600
02700 TXA ;Get high byte.
02800 EORZ TH
02900 STAZ TH
03000
03100 LSRA ;Shift by 4.
03200 RORZ TL
03300 LSRA
03400 RORZ TL
03500 LSRA
03600 RORZ TL
03700 LSRA
03800 RORZ TL
03900
04000 EORZ TH
04100 STAZ TH
04200 TYA
04300 EORZ TL
04400 EORZ TH ;Shift by 8.
04500 TAY ;Save low byte.
04600
04700 LDXZ TH ;Get high byte.
04800 BITZ TH
04900 BVC POS ;Check if negative.
05000 TXA
05100 ORAI 200 ;Extend sign.
05200 TAX
05300
05400 POS: TYA ;Returns with position in A, Y (low) and X (high).
05500 RTS
00100 ;Arithmetic routines.
00200 ;Enter with high byte in A, low in Y.
00300 ;Returns A = characteristic and sign, Y = mantissa.
00400 ;Clobbers X, LOGTMP, LOGTMP+1.
00500 LOG: STYZ LOGTMP ;Save the inputs.
00600 STAZ LOGTMP+1
00700
00800 LDXI 20+100 ;Init characteristic to 15.
00900 CMPI 0 ;Test sign of input.
01000 BPL POSIN
01100 SEC ;Negative. 2's complement it.
01200 LDAI 0
01300 SBCZ LOGTMP
01400 STAZ LOGTMP
01500 LDAI 0
01600 SBCZ LOGTMP+1
01700 POSIN: BNE NORML ;Is high byte zero?
01800 LDAZ LOGTMP ;Yes. Low byte?
01900 BEQ RTRN ;If so, return zero.
02000 LDYI 0 ;Low nonzero. Shift left one
02100 STYZ LOGTMP ;byte,
02200 LDXI 10+100 ;change characteristic to 7.
02300 NORML: DEX ;Normalize the number, counting the
02400 ASLZ LOGTMP ;characteristic down. When the
02500 ROLA ;first "1" shifts out, we've subtracted
02600 BCC NORML ;1 from the normalized number
02700 ASLZ LOGTMP ;(This rounds the result)
02800 ADCI =11 ;and are left with the fraction
02900 TAY ;Adding 11 to that is equivalent to
03000 TXA ;adding 0.043.
03100 ADCI 0 ;Propagate the carry into the
03200 ;characteristic.
03300 ASLA ;Insert the sign bit from the saved
03400 ASLZ LOGTMP+1;input.
03500 RORA
03600 RTRN: RTS ;Done.
03700
03800 ;Enter with sign and characteristic in A, mantissa in Y
03900 ;Returns 16-bit integer, low byte in Y, high in A.
04000 ;Clobbers X, LOGTMP, LOGTMP+1.
04100 EXP: STAZ LOGTMP+1;Save sign of input.
04200 ANDI 177 ;Mask it off.
04300 BEQ ZEROIN ;Zero characteristic returns
04400 TAX ;zero.
04500 TYA ;Get the mantissa...
04600 SEC
04700 SBCI =11 ;...subtract 0.043...
04800 STAZ LOGTMP ;(save this value)
04900 TXA ;...propagate the carry and get rid
05000 SBCI 100 ;of the XS-64 offset.
05100 BMI NEGIN ;If negative (value < 1.0)
05200 ;return zero.
05300 CMPI =15 ;Test for overflow (value>=2↑15
05400 BCS SATUR
05500 TAX ;...no. Number is in range.
05600 ADCI 370 ;Is characteristic below 8?
05700 BMI BLOATE
05800 TAX ;No. Reduce if by 8,
05900 JSR UNNORM ;unnormalize.
06000 BMI GETTMP ;Jump.
00100 BLOATE: JSR UNNORM ;Yes. Unnormalize, then
00200 ASLZ LOGTMP ;(round result)
00300 ADCI 0
00400 STAZ LOGTMP ;use result as low byte and
00500 LDAI 0 ;set high byte to zero.
00600
00700 GETTMP: LDYZ LOGTMP
00800 GTMP1: LDXZ LOGTMP+1;Test sign of input...
00900 BPL POSIGN
01000 STAZ LOGTMP+1;...negative. 2's complement
01100 LDAI 0 ;the result.
01200 SEC
01300 SBCZ LOGTMP
01400 TAY
01500 LDAI 0
01600 SBCZ LOGTMP+1
01700 POSIGN: RTS
01800
01900 NEGIN: LDAI 0 ;Set the result to zero if the
02000 ZEROIN: TAY ;input is negative.
02100 RTS
02200
02300 SATUR: LDYI 377 ;Saturate result to 2↑15 - 1 if
02400 STYZ LOGTMP ;input was 15 or more.
02500 LDAI 177
02600 BNE GTMP1 ;Jump.
02700
02800 UNNORM: LDAI 1 ;Unnormalize subroutine. Add 1
02900 BNE DECRX ;to the fraction. Jump.
03000
03100 SCALE: ASLZ LOGTMP ;Scale the fraction left by the
03200 ROLA ;amount of the characteristic.
03300 DECRX: DEX
03400 BPL SCALE
03500 RTS
00100 ;Enter with characteristic of multiplier in A,
00200 ;mantissa in Y, X pointing to a pair of base page
00300 ;locations containing the multiplicand (mantissa in the
00400 ;low byte).
00500 ;Returns the product in A and Y, same form as the
00600 ;multiplier. Leaves X unchanged. Clobbers LOGTMP and
00700 ;LOGTMP+1.
00800 MULTIP: PHA
00900 EORZX 1 ;Compute sign of result,
01000 STAZ LOGTMP+1 ;save it away.
01100 PLA
01200 ANDI 177 ;Mask off multiplier sign.
01300 BEQ ZEROIN ;If zero, return zero.
01400 STAZ LOGTMP
01500 TYA ;Add the two logarithms.
01600 CLC
01700 ADCZX 0
01800 TAY
01900 LDAZX 1
02000 ANDI 177 ;If multiplicand is zero,
02100 BEQ ZEROIN ;return a zero.
02200 ADCZ LOGTMP
02300 SEC
02400 SBCI 100 ;Correct the XS-64 offset.
02500 BPL INSIGN ;Result in range?
02600 ANDI 100 ;No. If underflow,
02700 BNE NEGIN ;return zero.
02800 LDAI 177 ;Overflow. Saturate to
02900 LDYI 377 ;highest magnitude.
03000
03100 INSIGN: ASLA ;Insert the sign of the result.
03200 ASLZ LOGTMP+1
03300 RORA
03400 RTS
03500
03600 ;Inverse function: 2's complement the magnitude part
03700 ;of a 15-bit logarithm.
03800 ;Enter with characteristic in A, mantissa in Y.
03900 ;Returns inverse in the same form. X unchanged.
04000 ;Clobbers LOGTMP and LOGTMP+1.
04100 INV: STYZ LOGTMP ;Pretty straightforward...
04200 STAZ LOGTMP+1
04300 SEC
04400 LDAI 0 ;Complement the number by
04500 SBCZ LOGTMP ;subtracting it from zero.
04600 TAY
04700 LDAI 0
04800 SBCZ LOGTMP+1
04900 JMP INSIGN ;Insert the original sign.
00100 ;DAC output table.
00200 LOC (.∨377)+1 ;For start of next page.
00300 VETBL: ;DAC output table.
00400 0 ↔ 20 ↔ 26 ↔ 33 ↔ 37 ↔ 43 ↔ 46 ↔ 50
00500 52 ↔ 54 ↔ 56 ↔ 57 ↔ 60 ↔ 62 ↔ 63 ↔ 64
00600 65 ↔ 66 ↔ 67 ↔ 70 ↔ 71 ↔ 72 ↔ 73 ↔ 74
00700 75 ↔ 76 ↔ 76 ↔ 77 ↔ 100 ↔ 101 ↔ 102 ↔ 103
00800 104 ↔ 104 ↔ 105 ↔ 106 ↔ 107 ↔ 107 ↔ 110 ↔ 111
00900 112 ↔ 112 ↔ 113 ↔ 114 ↔ 115 ↔ 115 ↔ 116 ↔ 117
01000 117 ↔ 120 ↔ 121 ↔ 121 ↔ 122 ↔ 123 ↔ 124 ↔ 124
01100 125 ↔ 126 ↔ 127 ↔ 127 ↔ 130 ↔ 131 ↔ 131 ↔ 132
01200 133 ↔ 133 ↔ 134 ↔ 135 ↔ 135 ↔ 136 ↔ 136 ↔ 137
01300 140 ↔ 140 ↔ 141 ↔ 142 ↔ 142 ↔ 143 ↔ 143 ↔ 144
01400 145 ↔ 145 ↔ 146 ↔ 146 ↔ 147 ↔ 150 ↔ 150 ↔ 151
01500 151 ↔ 152 ↔ 153 ↔ 153 ↔ 154 ↔ 154 ↔ 155 ↔ 156
01600 156 ↔ 157 ↔ 160 ↔ 160 ↔ 161 ↔ 161 ↔ 162 ↔ 162
01700 163 ↔ 164 ↔ 164 ↔ 165 ↔ 165 ↔ 166 ↔ 166 ↔ 167
01800 167 ↔ 170 ↔ 170 ↔ 171 ↔ 171 ↔ 172 ↔ 172 ↔ 173
01900 173 ↔ 174 ↔ 174 ↔ 175 ↔ 176 ↔ 176 ↔ 177 ↔ 177
02000
02100 200 ↔ 200 ↔ 200 ↔ 201 ↔ 201 ↔ 202 ↔ 203 ↔ 203
02200 204 ↔ 204 ↔ 205 ↔ 205 ↔ 206 ↔ 206 ↔ 207 ↔ 207
02300 210 ↔ 210 ↔ 211 ↔ 211 ↔ 212 ↔ 212 ↔ 213 ↔ 213
02400 214 ↔ 215 ↔ 215 ↔ 216 ↔ 216 ↔ 217 ↔ 217 ↔ 220
02500 221 ↔ 221 ↔ 222 ↔ 223 ↔ 223 ↔ 224 ↔ 224 ↔ 225
02600 226 ↔ 226 ↔ 227 ↔ 227 ↔ 230 ↔ 231 ↔ 231 ↔ 232
02700 232 ↔ 233 ↔ 234 ↔ 234 ↔ 235 ↔ 235 ↔ 236 ↔ 237
02800 237 ↔ 240 ↔ 241 ↔ 241 ↔ 242 ↔ 242 ↔ 243 ↔ 244
02900 244 ↔ 245 ↔ 246 ↔ 246 ↔ 247 ↔ 250 ↔ 250 ↔ 251
03000 252 ↔ 253 ↔ 253 ↔ 254 ↔ 255 ↔ 256 ↔ 256 ↔ 257
03100 260 ↔ 260 ↔ 261 ↔ 262 ↔ 262 ↔ 263 ↔ 264 ↔ 265
03200 265 ↔ 266 ↔ 267 ↔ 270 ↔ 270 ↔ 271 ↔ 272 ↔ 273
03300 273 ↔ 274 ↔ 275 ↔ 276 ↔ 277 ↔ 300 ↔ 301 ↔ 301
03400 302 ↔ 303 ↔ 304 ↔ 305 ↔ 306 ↔ 307 ↔ 310 ↔ 311
03500 312 ↔ 313 ↔ 314 ↔ 315 ↔ 317 ↔ 320 ↔ 322 ↔ 323
03600 325 ↔ 327 ↔ 331 ↔ 334 ↔ 340 ↔ 344 ↔ 351 ↔ 357
03700
03800 NMI ← START ;Reset??
03900 ;Interrupt vectors.
04000 LOC 177772
04100 NMI∧377
04200 (NMI⊗-10)∧377
04300 START∧377
04400 (START⊗-10)∧377
04500 TICK∧377
04600 (TICK⊗-10)∧377
04700 END